/*
 * Robot.java
 *
 * Created on November 7, 2007, 12:04 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package org.sunspotworld.robowulf;

import org.sunspotworld.robowulf.irsensor.IRSensor;
import org.sunspotworld.robowulf.irsensor.IRWheelController;
import org.sunspotworld.util.Units;

import com.sun.spot.resourcesharing.ResourceUnavailableException;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IOutputPin;
import com.sun.spot.sensorboard.io.IScalarInput;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.sensorboard.peripheral.LIS3L02AQAccelerometer;
import com.sun.spot.util.Utils;
import com.sun.squawk.util.IntHashtable;

/**
 * Implementation of IRobotController for the Robowulf robot platform
 * @author Douglas McClusky
 */
public class Robowulf implements IRobotController {
    public static final int FORWARD_ULTRASOUND_SENSOR = 0;

    private static Robowulf mrSpiffy = new Robowulf();

    private int units = Units.METERS;
    
    private IOutputPin[] hPins = EDemoBoard.getInstance().getOutputPins();
    private RobowulfServo[] wheels = {new RobowulfServo(hPins[0]), new RobowulfServo(hPins[1])};
    private ITriColorLED [] leds = EDemoBoard.getInstance().getLEDs();
    private IScalarInput[] adcs = EDemoBoard.getInstance().getScalarInputs();
    private LIS3L02AQAccelerometer acc = (LIS3L02AQAccelerometer)EDemoBoard.getInstance().getAccelerometer();
    
    private PINGUltrasoundSensor ping = new PINGUltrasoundSensor(EDemoBoard.getInstance().getIOPins()[EDemoBoard.D0],UltrasoundSensor.INCHES);
    private MaxSonarEZ1UltrasoundSensor maxSonar = new MaxSonarEZ1UltrasoundSensor(EDemoBoard.getInstance().getIOPins()[EDemoBoard.D0], UltrasoundSensor.INCHES);

    private IRSensor leftWheelIR;
    private IRSensor rightWheelIR;
    private IRWheelController irWheelController;

    private int sensorIndex = 1;
    private IntHashtable sensors = new IntHashtable();
    
    /**
     * @return the Singleton instance of Robowulf.
     */
    public static Robowulf getInstance() {
        return mrSpiffy;
    }
    
    /**
     * Since multiple competing IRobotControllers would be confusing, Robowulf must be accessed from its static instance.
     */
    private Robowulf() {
        acc.setScale(LIS3L02AQAccelerometer.SCALE_2G);
        
        String leftStopStr = System.getProperty("leftServoCenter");
        String rightStopStr = System.getProperty("rightServoCenter");
        int leftStop = leftStopStr != null ? Integer.parseInt(leftStopStr) : 1454;
        int rightStop = leftStopStr != null ? Integer.parseInt(rightStopStr) : 1454;
        
        int def[] = {leftStop, rightStop, 500, 500, Units.INCHES}; // Default configuration
        setCalibration(def);

        leftWheelIR = new IRSensor("left", adcs[2]);
        rightWheelIR = new IRSensor("right", adcs[3]);
        irWheelController = new IRWheelController(leftWheelIR, rightWheelIR, wheels[0], wheels[1]);
    }

    // CALIBRATION
    
    public void setCalibration(int cal[]) {
        units = cal[4];
        resetWheelCalibration(new int[] {cal[0], cal[1]}, new int[] {cal[2], cal[3]});
    }
  
    private void resetWheelCalibration(int[] stop, int[] halfwidths) {
    	wheels[0].setBounds(stop[0]+halfwidths[0],stop[0]-halfwidths[0]);
    	wheels[1].setBounds(stop[1]-halfwidths[1],stop[1]+halfwidths[1]);
    }

    // GET/SET UNITS
    
    public void setUnits(int units) {
        this.units = units;
    }

    public int getUnits() {
        return units;
    }

    // SERVOS
    
    public void stop() {
        brake(250);
        wheels[0].floatServo();
        wheels[1].floatServo();
    }

    public void brake(long time) {
        //set pulse widths
        wheels[0].brake();
        wheels[1].brake();
        Utils.sleep(time);
    }
    
    public void forward(float length) {
    	irWheelController.forward(length, units);
    }
        
    public void backward(float length) {
    	irWheelController.backward(length, units);
    }
    
    public void left(float degrees) {
    	irWheelController.left(degrees);
    }

    public void right(float degrees) {
    	irWheelController.right(degrees);
    }
    
    // SENSORS
    
    private double senseForwardRange() throws ResourceUnavailableException {
        if (maxSonar.testConnection()) {
            return maxSonar.getConvertedValue();
        }
        if (ping.testConnection()) {
            return ping.getConvertedValue();
        }
        throw new ResourceUnavailableException("No supported UltrasoundSensor detected.");
    }

    public boolean testIRSensor() {
        return leftWheelIR.testConnection() && rightWheelIR.testConnection();
    }

    /**
     * Register an IRobotSensor with Robowulf. <br>
     * A forward-facing ultrasound sensor is included with the base platform,
     * but other sensors can be interfaced through the power board and registered to Robowulf
     * via this method. These sensors can later be accessed via senseFrom(<index>), where <index> is a counter
     * that starts at 1 and increments and new sensors are added.<br>
     * <br>
     * Remember to include an appropriate resister in the signal connection from the sensor to the board.
     * @param sensor - the IRobobotSensor 
     */
    public void addSensor(IRobotSensor sensor) {
        sensors.put(sensorIndex, sensor);
        sensorIndex++;
    }
    
    /**
     * Returns the convereted sensor value of the indicated IRobotSensor.
     * @throws ResourceUnavailableException if the indicated IRobotSensor has not been registered to Robowulf, or
     * if it cannot be detected.
     */
    public synchronized double senseFrom(int index) throws ResourceUnavailableException {
        if (index == FORWARD_ULTRASOUND_SENSOR) {
            return senseForwardRange();
        }
        if (index < this.sensorIndex) {
            return ((IRobotSensor)sensors.get(index)).getConvertedValue();
        }
        throw new ResourceUnavailableException("Requested IRobotSensor not registered with this Robowulf.");
    }
}
